#include "State_FixedStand.h"
#include "Dog.h"

#define KP1 6.0f
#define KD1 1.3f

void FixedStand_Enter(void)
{
    UART_Print("fixedStand enter\n");

    /*HT_SetPosition(dog.legs[0].motorF, ANGLE_F, KP1, KD1, 0);
    HT_SetPosition(dog.legs[1].motorF, -ANGLE_F, KP1, KD1, 0);
    HT_SetPosition(dog.legs[2].motorF, -ANGLE_F, KP1, KD1, 0);
    HT_SetPosition(dog.legs[3].motorF, ANGLE_F, KP1, KD1, 0);

    HT_SetPosition(dog.legs[0].motorB, -ANGLE_B, KP1, KD1, 0);
    HT_SetPosition(dog.legs[1].motorB, ANGLE_B, KP1, KD1, 0);
    HT_SetPosition(dog.legs[2].motorB, ANGLE_B, KP1, KD1, 0);
    HT_SetPosition(dog.legs[3].motorB, -ANGLE_B, KP1, KD1, 0);*/
}

#define KP2_1 8.0f
#define KD2_1 1.3f
#define KP2_2 8.0f
#define KD2_2 1.0f

void FixedStand_Run(void)
{
    /*if (HT_ArrivalPos(dog.legs[0].motorF, 0.5) &&
        HT_ArrivalPos(dog.legs[1].motorF, 0.5) &&
        HT_ArrivalPos(dog.legs[2].motorF, 0.5) &&
        HT_ArrivalPos(dog.legs[3].motorF, 0.5))
    {
        HT_SetPosition(dog.legs[0].motorF, STARTANGLE_F, KP2_1, KD2_1, 0);
        HT_SetPosition(dog.legs[1].motorF, -STARTANGLE_F, KP2_1, KD2_1, 0);
        HT_SetPosition(dog.legs[2].motorF, -STARTANGLE_F, KP2_2, KD2_2, 0);
        HT_SetPosition(dog.legs[3].motorF, STARTANGLE_F, KP2_2, KD2_2, 0);

        HT_SetPosition(dog.legs[0].motorB, STARTANGLE_B, KP2_1, KD2_1, 0);
        HT_SetPosition(dog.legs[1].motorB, -STARTANGLE_B, KP2_1, KD2_1, 0);
        HT_SetPosition(dog.legs[2].motorB, -STARTANGLE_B, KP2_2, KD2_2, 0);
        HT_SetPosition(dog.legs[3].motorB, STARTANGLE_B, KP2_2, KD2_2, 0);
        
    }*/
    HT_SetPosition(dog.legs[0].motorF, ANGLE_F, KP2_1, KD2_1, 0);
    HT_SetPosition(dog.legs[1].motorF, -ANGLE_F, KP2_1, KD2_1, 0);
    HT_SetPosition(dog.legs[2].motorF, -ANGLE_F, KP2_2,  KD2_2, 0);
    HT_SetPosition(dog.legs[3].motorF, ANGLE_F, KP2_2,  KD2_2, 0);

    HT_SetPosition(dog.legs[0].motorB, -ANGLE_B, KP2_1,KD2_1, 0);
    HT_SetPosition(dog.legs[1].motorB, ANGLE_B, KP2_1, KD2_1, 0);
    HT_SetPosition(dog.legs[2].motorB, ANGLE_B, KP2_2,  KD2_2, 0);
    HT_SetPosition(dog.legs[3].motorB, -ANGLE_B, KP2_2,  KD2_2, 0);
}

void FixedStand_Exit(void)
{
    UART_Print("fixedStand exit\n");
}